from mpython import *
from machine import I2C,Pin 
import machine
import time
i2c = I2C(scl=Pin(22), sda=Pin(23), freq=100000)
class Tinybit(object):

    
    def __init__(self):

        self._pwm_addr = 0x01
        self._MOTOR = 0x02
        self._rgb_addr = 0x01
        

    def setMotorPwm(self, index, speed1, speed2):
        speed1 = abs(speed1)
        speed2 = abs(speed2)
        if(speed1 > 255):
            speed1 = 255
        if(speed2 > 255):
            speed2 = 255
        buf = [0x02, 0, 0, 0, 0]
        if((index > 6) or (index < 1)):
            return
        if(index == 1):
            buf[1] = speed1
            buf[3] = speed2
            send = bytes(buf)
            i2c.writeto(0x01, send)
        elif(index == 2):
            buf[2] = speed1
            buf[4] = speed2 
            send = bytes(buf)
            i2c.writeto(0x01, send)
        elif(index == 3):
            buf[3] = speed2 
            send = bytes(buf)
            i2c.writeto(0x01, send)
        elif(index == 4):
            buf[1] = speed1
            send = bytes(buf)
            i2c.writeto(0x01, send)
        elif(index == 5):
            buf[2] = speed1
            buf[3] = speed2 
            send = bytes(buf)
            i2c.writeto(0x01, send)
        elif(index == 6):
            buf[1] = speed1
            buf[4] = speed2 
            send = bytes(buf)
            i2c.writeto(0x01, send)
 
 
    def Stop(self):
        buf = [0x02, 0, 0, 0, 0]
        send = bytes(buf)
        i2c.writeto(0x01, send)
 
 
  
    def Run(self, index):
        self.setMotorPwm(index, 255, 255)
    
 
 
    def Run_Speed(self, index, speed):
         self.setMotorPwm(index, speed, speed)       
    

 
    def Run_Speed2(self, index, speed1, speed2):
        self.setMotorPwm(index, speed1, speed2)        
    
    
 
    def HeadRGB(self, red, green, blue):
        red = abs(red)
        green = abs(green)
        blue = abs(blue)
        if(red > 255):
            red = 255
        if(green > 255):
            green = 255
        if(blue > 255):
           blue = 255
        buf = [0x01, 0, 0, 0]
        buf[1] = red
        buf[2] = green 
        buf[3] = blue
        send = bytes(buf)        
        i2c.writeto(0x01, send)
    

    def headlight_close(self):
        i2c.writeto(self._pwm_addr, (bytearray([self._rgb_addr, 0, 0, 0])))
    
        
    def ultrasonic(self,int_unit):
        echoTimeout = 23200
        trigPin = Pin(Pin.P16, Pin.OUT)
        echoPin = Pin(Pin.P15, Pin.IN)
        trigPin.value(0) 
        trigPin.value(1)
        time.sleep_us(10)
        trigPin.value(0)
        pulseTime = machine.time_pulse_us(echoPin, 1, echoTimeout)
        try:
            if pulseTime > 0:
                if int_unit == 0:
                    distance = round((pulseTime / 58),1)
                    str_unit = 'cm'
                else:
                    distance = round((pulseTime / 147.32),2)
                    str_unit = 'in'
                   
            if distance > 0:
                print('Distance: ', distance, str_unit)
                return distance
        except:
            print('Out of Ultrasonic Measurement Range.')
            return -1
            
            
            
    def get_tracking(self, side):
        pin_l = Pin(Pin.P13, Pin.IN, None)
        pin_r = Pin(Pin.P14, Pin.IN, None)      
        tracking_l = pin_l.value()
        tracking_r = pin_r.value()
        if side == 0:
            return tracking_l
        elif side == 1:
            return tracking_r
        else:
            return -1


    def is_track_side(self, side, state):
        if self.get_tracking(side) == state:
            return True
        else:
            return False